/*
 * Copyright (c) 2025 STMROS Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stmconn/stm_dialect.hpp>
#include <cstring>

namespace stmconn {

// 序列化辅助函数
template<typename T>
void serialize_field(const T &value, std::vector<uint8_t> &buffer) {
    const uint8_t *bytes = reinterpret_cast<const uint8_t*>(&value);
    buffer.insert(buffer.end(), bytes, bytes + sizeof(T));
}

// 反序列化辅助函数
template<typename T>
bool deserialize_field(const std::vector<uint8_t> &buffer, size_t &offset, T &value) {
    if (offset + sizeof(T) > buffer.size()) {
        return false;
    }
    memcpy(&value, buffer.data() + offset, sizeof(T));
    offset += sizeof(T);
    return true;
}

// 心跳消息序列化
size_t serialize_heartbeat(const Heartbeat &msg, std::vector<uint8_t> &buffer) {
    size_t start_size = buffer.size();
    serialize_field(msg.system_id, buffer);
    serialize_field(msg.component_id, buffer);
    serialize_field(msg.mode, buffer);
    serialize_field(msg.state, buffer);
    serialize_field(msg.time_boot_ms, buffer);
    return buffer.size() - start_size;
}

// 命令消息序列化
size_t serialize_command(const Command &msg, std::vector<uint8_t> &buffer) {
    size_t start_size = buffer.size();
    serialize_field(msg.command_id, buffer);
    serialize_field(msg.param1, buffer);
    serialize_field(msg.param2, buffer);
    serialize_field(msg.param3, buffer);
    serialize_field(msg.param4, buffer);
    serialize_field(msg.param5, buffer);
    serialize_field(msg.param6, buffer);
    serialize_field(msg.param7, buffer);
    return buffer.size() - start_size;
}

// 状态消息序列化
size_t serialize_status(const Status &msg, std::vector<uint8_t> &buffer) {
    size_t start_size = buffer.size();
    serialize_field(msg.system_status, buffer);
    serialize_field(msg.battery_status, buffer);
    serialize_field(msg.error_code, buffer);
    serialize_field(msg.voltage, buffer);
    serialize_field(msg.temperature, buffer);
    return buffer.size() - start_size;
}

// 传感器数据消息序列化
size_t serialize_sensor_data(const SensorData &msg, std::vector<uint8_t> &buffer) {
    size_t start_size = buffer.size();
    serialize_field(msg.time_usec, buffer);
    serialize_field(msg.accel_x, buffer);
    serialize_field(msg.accel_y, buffer);
    serialize_field(msg.accel_z, buffer);
    serialize_field(msg.gyro_x, buffer);
    serialize_field(msg.gyro_y, buffer);
    serialize_field(msg.gyro_z, buffer);
    serialize_field(msg.mag_x, buffer);
    serialize_field(msg.mag_y, buffer);
    serialize_field(msg.mag_z, buffer);
    serialize_field(msg.pressure, buffer);
    serialize_field(msg.altitude, buffer);
    serialize_field(msg.temperature, buffer);
    return buffer.size() - start_size;
}

// 电机控制消息序列化
size_t serialize_motor_control(const MotorControl &msg, std::vector<uint8_t> &buffer) {
    size_t start_size = buffer.size();
    serialize_field(msg.motor_id, buffer);
    serialize_field(msg.throttle, buffer);
    serialize_field(msg.pwm_value, buffer);
    return buffer.size() - start_size;
}

// 心跳消息反序列化
bool deserialize_heartbeat(const std::vector<uint8_t> &buffer, Heartbeat &msg) {
    size_t offset = 0;
    if (!deserialize_field(buffer, offset, msg.system_id)) return false;
    if (!deserialize_field(buffer, offset, msg.component_id)) return false;
    if (!deserialize_field(buffer, offset, msg.mode)) return false;
    if (!deserialize_field(buffer, offset, msg.state)) return false;
    if (!deserialize_field(buffer, offset, msg.time_boot_ms)) return false;
    return true;
}

// 命令消息反序列化
bool deserialize_command(const std::vector<uint8_t> &buffer, Command &msg) {
    size_t offset = 0;
    if (!deserialize_field(buffer, offset, msg.command_id)) return false;
    if (!deserialize_field(buffer, offset, msg.param1)) return false;
    if (!deserialize_field(buffer, offset, msg.param2)) return false;
    if (!deserialize_field(buffer, offset, msg.param3)) return false;
    if (!deserialize_field(buffer, offset, msg.param4)) return false;
    if (!deserialize_field(buffer, offset, msg.param5)) return false;
    if (!deserialize_field(buffer, offset, msg.param6)) return false;
    if (!deserialize_field(buffer, offset, msg.param7)) return false;
    return true;
}

// 状态消息反序列化
bool deserialize_status(const std::vector<uint8_t> &buffer, Status &msg) {
    size_t offset = 0;
    if (!deserialize_field(buffer, offset, msg.system_status)) return false;
    if (!deserialize_field(buffer, offset, msg.battery_status)) return false;
    if (!deserialize_field(buffer, offset, msg.error_code)) return false;
    if (!deserialize_field(buffer, offset, msg.voltage)) return false;
    if (!deserialize_field(buffer, offset, msg.temperature)) return false;
    return true;
}

// 传感器数据消息反序列化
bool deserialize_sensor_data(const std::vector<uint8_t> &buffer, SensorData &msg) {
    size_t offset = 0;
    if (!deserialize_field(buffer, offset, msg.time_usec)) return false;
    if (!deserialize_field(buffer, offset, msg.accel_x)) return false;
    if (!deserialize_field(buffer, offset, msg.accel_y)) return false;
    if (!deserialize_field(buffer, offset, msg.accel_z)) return false;
    if (!deserialize_field(buffer, offset, msg.gyro_x)) return false;
    if (!deserialize_field(buffer, offset, msg.gyro_y)) return false;
    if (!deserialize_field(buffer, offset, msg.gyro_z)) return false;
    if (!deserialize_field(buffer, offset, msg.mag_x)) return false;
    if (!deserialize_field(buffer, offset, msg.mag_y)) return false;
    if (!deserialize_field(buffer, offset, msg.mag_z)) return false;
    if (!deserialize_field(buffer, offset, msg.pressure)) return false;
    if (!deserialize_field(buffer, offset, msg.altitude)) return false;
    if (!deserialize_field(buffer, offset, msg.temperature)) return false;
    return true;
}

// 电机控制消息反序列化
bool deserialize_motor_control(const std::vector<uint8_t> &buffer, MotorControl &msg) {
    size_t offset = 0;
    if (!deserialize_field(buffer, offset, msg.motor_id)) return false;
    if (!deserialize_field(buffer, offset, msg.throttle)) return false;
    if (!deserialize_field(buffer, offset, msg.pwm_value)) return false;
    return true;
}

} // namespace stmconn